/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStationLCD;

/**
 *
 * @author Ben_R_R
 */
public class DriveStationFF {

    private DriverStationLCD dsLCD;

    /* We have to store the old lines to simulate console style output on the
     driver station. 5 is the bottom, 0 is the top */
    private String[] dSLineBuffer = new String[6];
    // number of spaces to clear a line.
    private String clearLine = "                     ";
    // current open line.
    private int openLine = 0;

    private DriverStationLCD.Line[] dSLines = new DriverStationLCD.Line[6];
    private DriverStation dash;

    /** used to send non-text data to the drive station. */
    private DStation driveStationData;

    public void update(){
       driveStationData.updateDashboard();
       dsLCD.updateLCD();
    }

    /**
     * Initializes the Driver Station, call from robotInit.
     */
    public DriveStationFF(){
        dash = DriverStation.getInstance();
        dsLCD = DriverStationLCD.getInstance();
        driveStationData = new DStation();

        dSLines[5] = DriverStationLCD.Line.kUser6;
        dSLines[4] = DriverStationLCD.Line.kUser5;
        dSLines[3] = DriverStationLCD.Line.kUser4;
        dSLines[2] = DriverStationLCD.Line.kUser3;
        dSLines[1] = DriverStationLCD.Line.kUser2;
        dSLines[0] = DriverStationLCD.Line.kMain6;

        for(int i = 0; i < dSLineBuffer.length; i++){
            dSLineBuffer[i] = clearLine;
        }
    }

    /**
     * Prints a message console style to the drive station user messages.
     * @param msg
     */
    public void dSPrint(String msg){

        if(openLine == 6){
            for(int i = 1; i < dSLineBuffer.length; i++){
                dSLineBuffer[i-1] = dSLineBuffer[i];
            }
            openLine = 5;
        }

        dSLineBuffer[openLine] = padTo21(msg);
        openLine++;

        if(dsLCD != null){
            for(int l = 0; l < dSLines.length; l++ ){
                if (dSLines[l] != null){
                    dsLCD.println(dSLines[l], 1, dSLineBuffer[l]);
                }else{
                    // Should never get here, but just to be safe.
                    System.out.println("Null Line: " + l);
                }
            }
            dsLCD.updateLCD();
        } else {

            System.out.println("No DriverStationLCD!");
        }
    }

    /**  pad the given string to 21 characters by adding spaces to the end*/
    private String padTo21(String input){
        String out = input;

        while(out.length() < 21){ out = out + " ";}
        return out;
    }
}
